#include "calibration.h"

calibration::calibration()
{
    test_image_points=cv::Mat_<double>(2,1);
    test_world_points=cv::Mat_<double>(2,1);
}

void calibration::getH()
{
    H = cv::findHomography(world_points,image_points,0);

    cv::Mat matR(H,cv::Range(0,2),cv::Range(0,2));
    cv::Mat matT(H,cv::Range(0,2),cv::Range(2,3));
    R=matR;
    T=matT;
    cout << "H:\n" << H << endl;
    cout << "R:\n" << R << endl;
    cout << "T:\n" << T << endl;

}

cv::Mat calibration::valuate()
{
    cv::Mat out;
    out=R*test_world_points + T;
    cout << "out:\n" << out << endl;
    cout << "test_image_points:\n" << test_image_points << endl;
    return out;
}

void calibration::read_points()
{
    FILE* fp1, *fp2;
    world_points.clear();
    image_points.clear();
    fp1 = fopen("./input_image_points.txt","r");
    fp2 = fopen("./input_world_points.txt","r");
    float p_x,p_y;
    //计数
    int num_points=0;

    if(fp2 != nullptr)
    {
        fscanf(fp2,"%d",&num_points);
        std::cout<<"Points Num:"<<num_points<<std::endl;

        for(int i=0;i<num_points;i++)
        {
            fscanf(fp2,"%f %f",&p_x,&p_y);
            cv::Point2f src_point(p_x,p_y);
            if(i==num_points-1){
                test_world_points.at<double>(0,0)=p_x;
                test_world_points.at<double>(1,0)=p_y;
            }else{
                world_points.push_back(src_point);
            }
        }
        fclose(fp2);
    } else
    {
        printf("input_image_points.txt read error!\n");
        abort();
    }

    if(fp1 != nullptr)
    {
        for(int i=0;i<num_points;i++)
        {
            fscanf(fp1,"%f %f",&p_x,&p_y);
            cv::Point2f src_point(p_x,p_y);
            if(i==num_points-1){
                test_image_points.at<double>(0,0)=p_x;
                test_image_points.at<double>(1,0)=p_y;
            }else{
                image_points.push_back(src_point);

            }
        }
        fclose(fp1);
    }
    else
    {
        printf("input_image_points.txt read error!\n");
        abort();
    }
    cout << "input world_points:\n";
    for(size_t i = 0;i<world_points.size();++i)
    {
        cout << world_points[i] << endl;
    }
    cout << "input image_points:\n";
    for(size_t i = 0;i<world_points.size();++i)
    {
        cout << image_points[i] << endl;
    }
}
